#include "cvxCapture.h"
#include "commonHead.h"
#include "cvxIo.h"
#include <string>
#include "CCamera.h"
#include "cvxVideo.h"
#include "cvxSf.h"

using namespace std;



//test circle checker board
#if 0
//test circles check;
//result no good
//square 63ms, circle 281ms 
// find4QuadCornerSubpix is more accurate than cornerSubPix, but more slow 1000ms
int main()
{
	IplImage *imageA = cvLoadImage("checkerboard\\chessboard_16_10_75in_circle.png", 1);
	vector<Point2f> centerPts;
	double tt = clock();	
	bool isOk = cv::findCirclesGrid(Mat(imageA), cvSize(8, 10), centerPts, CALIB_CB_ASYMMETRIC_GRID);
	printf(".75 in cost time = %f\n", clock()-tt);
	if (isOk)
	{
		cv::drawChessboardCorners(Mat(imageA), cvSize(8, 10), centerPts, true);
		cvShowImage(".75 circles center", imageA);		
	}

	imageA = cvLoadImage("checkerboard\\chessboard_16_10_1375in_circle_rotate.png", 1);
	tt = clock();
	centerPts.clear();
	isOk = cv::findCirclesGrid(Mat(imageA), cvSize(8, 10), centerPts, CALIB_CB_ASYMMETRIC_GRID);
	printf("1.375 in cost time = %f\n", clock()-tt);
	if (isOk)
	{
		cv::drawChessboardCorners(Mat(imageA), cvSize(8, 10), centerPts, true);
		cvShowImage("1.375 circles center", imageA);		
	}

	cvWaitKey(0);
	return 1;

	imageA = cvLoadImage("checkerboard\\chessboard_16_10.png", 1);
	tt = clock();
	centerPts.clear();
	isOk = cv::findChessboardCorners(Mat(imageA), cvSize(15, 9), centerPts);
	if (isOk)
	{
	//	isOk = cv::find4QuadCornerSubpix()
		IplImage *grayImage = cvCreateImage(cvGetSize(imageA), IPL_DEPTH_8U, 1);
		cvCvtColor(imageA, grayImage, CV_BGR2GRAY);
		vector<Point2f> subCornerA = centerPts;
		tt = clock();
		cv::cornerSubPix(Mat(grayImage), subCornerA, Size(11, 11), Size(-1,-1), 
						 TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
		cout<<"cornerSubPix cost time = "<<clock() - tt<<endl;
		cv::drawChessboardCorners(Mat(imageA), cvSize(15, 9), subCornerA, true);
	//	cvShowImage("cornerSubPix", imageA);

		imageA = cvLoadImage("checkerboard\\chessboard_16_10.png", 1);
		
		vector<Point2f> subCornerB = centerPts;
		tt = clock();
		cv::cornerSubPix(Mat(grayImage), subCornerB, Size(11, 11), Size(-1,-1), 
			TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
		cv::find4QuadCornerSubpix(Mat(grayImage), subCornerB, cvSize(5, 5));
		cout<<"find4QuadCornerSubpix cost time = "<<clock() - tt<<endl;

		vector<Point2f> subCornerC = centerPts;
		cv::find4QuadCornerSubpix(Mat(grayImage), subCornerC, cvSize(5, 5));

		vector<Point2f> subA_C;
		vector<Point2f> subB_C;
		for (int i = 0; i<subCornerA.size(); ++i)
		{
			Point2f difAC = subCornerA[i] - subCornerC[i];
			Point2f difBC = subCornerB[i] - subCornerC[i];
			subB_C.push_back(difBC);
			subA_C.push_back(difAC);
		}


		cout<<"square cost time = "<<clock()-tt<<endl;
		cv::drawChessboardCorners(Mat(imageA), cvSize(15, 9), subCornerB, true);
		//cvShowImage("find4QuadCornerSubpix center", imageA);	
	}


	cvWaitKey(0);

	return 1;
}
#endif



#if 0
//test median{d(Hm, m')} method in 
//Visual Modeling with a Hand-Held Camera
//it works
int main()
{

	string botVideoFileName = string("video\\20111117\\rotate-z90\\960\\bot_z90_960.avi");
	string topVideoFileName = string("video\\20111117\\rotate-z90\\960\\top_z90_960.avi");

	string videoFileName;
	vector<int> Idx;
	vector<vector<Point2f> > botChessboardPos;
	vector<vector<Point2f> > topChessboardPos;
	CvSize chessboardSize;
	CChessBoard board(cvSize(15, 9), 0.040);

//	CvxSF::saveStereoChessboard(botVideoFileName, topVideoFileName, string("bot_top_z90_ckb_pos.txt"), 1, cvSize(15, 9), true);

	CvxSF::loadStereoChessboard(string("video\\20111117\\rotate-z90\\960\\bot_top_z90_ckb_pos.txt"), botVideoFileName, topVideoFileName, 

		Idx, chessboardSize, botChessboardPos, topChessboardPos);

#if 1

	FILE *tPf = fopen("4_20_sequence.txt", "w");
	for (int k = 4; k<20; k++)
	{
		CvCapture *pCap = cvCaptureFromFile(botVideoFileName.c_str());
		assert(pCap);
		CvxFileCapture fileCapture(pCap, 1);

		vector<vector<Point2f>> botCandidateVec;
		int candinateNum = botChessboardPos.size()/k;
		for (int i = 0; i<botChessboardPos.size(); ++i)
		{
			if (i%candinateNum == 0)
			{
			//	botCandidateVec.push_back(botChessboardPos[i]);
				IplImage *grab = fileCapture.GetIdxFrame(Idx[i]);
				assert(grab);
				IplImage *image = cvCloneImage(grab);
				assert(image);
				char buf[255] = {NULL};
				sprintf(buf, "%d", i);
				string name(buf);

				vector<Point2f> pts;
				bool isOk = CvxSF::GetChessboard(image, board.size, false, pts);
				if (isOk)
				{
					if (pts[0].y > pts[board.size.width].y)
					{
						std::reverse(pts.begin(), pts.end());
					}

					cv::drawChessboardCorners(Mat(image), board.size, pts, true);
					botCandidateVec.push_back(pts);
				}
			//	cvShowImage(name.c_str(), image);
			//	cvWaitKey(1);
				char buf2[255] = {NULL};
				sprintf(buf2, "%d", k);
				string orgName = string("save_image\\") + string(buf2) + string("\\0_") + name + string(".png");
				cvSaveImage(orgName.c_str(), grab);


				name = string("save_image\\") + string(buf2) + string("\\") + name + string(".png");
				cvSaveImage(name.c_str(), image);
				
				cvReleaseImage(&image);
			}			
		}
		cvReleaseCapture(&pCap);

		CCamera botCamera;
		botCamera.setFeature2d(botCandidateVec);
		botCamera.setCorners3d(board);
		botCamera.setImageSize(cvSize(960, 540)); //CV_CALIB_FIX_PRINCIPAL_POINT
		botCamera.calib(board, CV_CALIB_ZERO_TANGENT_DIST|CV_CALIB_FIX_K3, true);
		Mat intri = botCamera.getIntrinsicMatrix(); 
		Mat distortion = botCamera.getDistortionCoeffs(); 
		fprintf(tPf, "%d %f %f %f %f %f %f %f\n\n", k, intri.at<double>(0, 0), intri.at<double>(1, 1), intri.at<double>(0, 2),
					  intri.at<double>(1, 2), distortion.at<double>(0, 0), distortion.at<double>(1, 0),
					  intri.at<double>(1, 1)/intri.at<double>(0, 0));

	//	botCamera.saveCamera(string("bot_960_10.txt"));
	//	botCamera.saveText(string("bot_z90_960.avi"), string("bottom960_cam_10.txt")); //	15_9_top960.txt
	}
	fclose(tPf);

	cvWaitKey(0);

	return 1;
#endif

	CvCapture *botCap = cvCaptureFromFile(botVideoFileName.c_str());
	CvCapture *topCap = cvCaptureFromFile(topVideoFileName.c_str());

	int frame = 0;
	IplImage *leftImage = cvQueryFrame(botCap);
	IplImage *rightImage = cvQueryFrame(topCap);
	IplImage *leftShow = cvCloneImage(leftImage);
	IplImage *rightShow = cvCloneImage(rightImage);

	vector<Point2f> firstPts = botChessboardPos[0];
	vector<double> disVec;

	for (int i = 0; i<Idx.size(); ++i)
	{
		while(leftImage && rightImage && frame <Idx[i])
		{
			leftImage = cvQueryFrame(botCap);
			rightImage = cvQueryFrame(topCap);
			cvShowImage("left", leftImage);
			cvShowImage("right", rightImage);
			cvWaitKey(10);			
			frame++;
		}
		if (!leftImage || !rightImage)
		{
			break;
		}
		cvCopy(leftImage, leftShow);
		cvCopy(rightImage, rightShow);

		if(0)
		{
			//computer median{ d(Hm, m')}
			Mat H;
			H = cv::findHomography(firstPts, botChessboardPos[i]);
			vector<double> distance;
			for (int j = 0; j<firstPts.size(); ++j)
			{
				double _p1Mat[3];
				Point2f p1 = firstPts[j];
				Point2f p2 = botChessboardPos[i][j];
				Mat p1Mat = Mat(3, 1, CV_64F, _p1Mat);
				p1Mat.at<double>(0, 0) = p1.x;
				p1Mat.at<double>(1, 0) = p1.y;
				p1Mat.at<double>(2, 0) = 1.0;
				Mat h_src = H * p1Mat;
				h_src = h_src/h_src.at<double>(2, 0);
				double dis = (p2.x - h_src.at<double>(0, 0)) * (p2.x - h_src.at<double>(0, 0))
					+ (p2.y - h_src.at<double>(1, 0)) * (p2.y - h_src.at<double>(1, 0));
				distance.push_back(dis);
			}
			std::sort(distance.begin(), distance.end());
			double median_d = distance[distance.size()/2];
			disVec.push_back(median_d);
			//
			char botBuf[255] = {NULL};
			const CvFont font = cvFont(2.0, 1);
			sprintf(botBuf, "distance: %f", median_d);
			cvPutText(leftShow, botBuf, cvPoint(0, 540-40), &font, cvScalar(0, 0, 255));
		}
		
		cv::drawChessboardCorners(Mat(leftShow), board.size, botChessboardPos[i], true);
		cv::drawChessboardCorners(Mat(rightShow), board.size, topChessboardPos[i], true);
		cvShowImage("left", leftShow);
		cvShowImage("right", rightShow);
		if (i == 112)
		{
			cvWaitKey(0);
		}
		leftImage = cvQueryFrame(botCap);
		rightImage = cvQueryFrame(topCap);
		frame++;
		cvWaitKey(10);
	}

	//smooth
	for (int i = 0; i<disVec.size(); ++i)
	{
		if (i != 0 && i != disVec.size()-1)
		{
			disVec[i] = (2*disVec[i] + disVec[i-1] + disVec[i+1])/4.0;
		}
	}

	FILE *pf = fopen("median_p.txt", "w");
	for (int i = 0; i<disVec.size(); ++i)
	{
		fprintf(pf, "%f %f ", i*1.0, disVec[i]);
	}
	fclose(pf);
	cvWaitKey(0);

	//
	return 1;
}
#endif

#if 0
int main()
{
	CvCapture *cap = cvCaptureFromFile("video\\15_9\\bottom_23345300_15_9_960.avi");
	assert(cap);
	int isoK = cvSetCaptureProperty(cap, CV_CAP_PROP_POS_FRAMES , 1);
	IplImage *imageA = cvQueryFrame(cap);
	cvShowImage("1", imageA);
	isoK = cvSetCaptureProperty(cap, CV_CAP_PROP_POS_FRAMES, 100);
	IplImage *imageB = cvQueryFrame(cap);
	cvShowImage("100", imageB);
	cvWaitKey(0);
	return 0;
}
#endif

#if 0
// save and load checkboard
int main()
{

	vector<string> fileNames;
	vector<IplImage *> imageVec;
	bool isOk = CvxIo::travel_dir("save_image\\15_9\\top\\960\\*.png", fileNames);

	for (int i = 0; i<fileNames.size(); ++i)
	{
		string fname = string("save_image\\15_9\\top\\960\\") + fileNames[i];
		IplImage *image = cvLoadImage(fname.c_str(), 1);
		assert(image);
		imageVec.push_back(image);
	}

	CChessBoard board(cvSize(15, 9), 0.025);

	{

		// 		FILE *pf = fopen("save_image\\15_9\\top\\960\\points.txt", "w");
		// 		fprintf(pf, "%d\n%d %d\n", imageVec.size(), board.size.width, board.size.height);
		// 		CvxSF::findSaveChessboard(imageVec, pf, board.size);
		// 		fclose(pf);
	}

	FILE *pf = fopen("save_image\\15_9\\top\\960\\points.txt", "r");
	vector<int> idx;
	vector<vector<Point2f> > chessboardPos;
	CvSize size;
	CvxSF::loadChessboard(pf, idx, chessboardPos, size);
	fclose(pf);

	for (int i = 0; i<idx.size(); ++i)
	{
		IplImage *image = imageVec[idx[i]];
		cv::drawChessboardCorners(Mat(image), board.size, chessboardPos[i], true); 		
		cvShowImage("checkboard", image);
		cvWaitKey(100);
	}


	// 	for (int i = 0; i<imageVec.size(); ++i)
	// 	{
	// 		IplImage *image = imageVec[i];
	// 		vector<Point2f> pts;
	// 		bool isOk = CvxSF::GetChessboard(image, board.size, false, pts);
	// 		if (isOk)
	// 		{
	// 			cv::drawChessboardCorners(Mat(image), board.size, pts, true);
	// 		}
	// 		cvShowImage("checkboard", image);
	// 		cvWaitKey(100);
	// 	}



	cvWaitKey(0);
}

#endif


#if 0
//get small size video
int main()
{
	// bottom_23345300_15_9.MXF
	// top_23345209_15_9.MXF
	CvSize size = cvSize(320, 240);	
// 	CvCapture *cp = cvCaptureFromFile("pure_multi.avi");
// 	assert(cp);
// 	IplImage *frame = cvQueryFrame(cp);
// 	int w = frame->width;
// 	int h = frame->height;
//	printf("w = %d\nh = %d\n", w, h);
	CvxVideo::writeToAVI(string("pure_multi.avi"), 
						 string("pure_multi_1.avi"),
						 24, size, 0, 3000, -2);
	cvWaitKey(0);
	return 1;
}
#endif



#if 0
//calibration camera seperately
int main()
{

	vector<string> fileNames;
	bool isOk = CvxIo::travel_dir("save_image\\15_9\\left\\*.png", fileNames);
	ImageSequenceCapture pCap;
	for (int i = 0; i<fileNames.size(); ++i)
	{
		string fname = string("save_image\\15_9\\left\\") + fileNames[i];
		IplImage *image = cvLoadImage(fname.c_str(), 1);
		assert(image);
		pCap.push_back(image);
	}
	
	CChessBoard board(cvSize(15, 9), 0.025);

	CCamera iCamera;	
	iCamera.captureFeature(&pCap, board, fileNames.size());
	iCamera.setCorners3d(board);
	// CV_CALIB_FIX_K3| CV_CALIB_FIX_K4|CV_CALIB_FIX_K5  CV_CALIB_FIX_PRINCIPAL_POINT
	iCamera.calib(board, CV_CALIB_FIX_PRINCIPAL_POINT|CV_CALIB_ZERO_TANGENT_DIST|CV_CALIB_FIX_K3, true);
	iCamera.saveCamera(string("left.txt"));
	iCamera.saveText(string("15_9_bottom_960.txt"), string("15_9_bottom960.txt")); //	15_9_top960.txt
	//iCamera.printReprojectionError();	

	cvWaitKey(0);

}
#endif


#if 0
//extract image from videos
int main()
{
	double tt = clock();

	// bottom_23345300_15_9.MXF
	CvCapture *bottomC  = cvCaptureFromFile("video\\15_9\\bottom_23345300_15_9.MXF"); 
	CvCapture *topC     = cvCaptureFromFile("video\\15_9\\top_23345209_15_9.MXF");

	int num = 1;
	while(num--)
	{
		int isOk = cvGrabFrame(topC);		
	}
	
	int w = 1920;
	int h = 1080;
	IplImage *leftShow = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 3);
	IplImage *rightShow = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 3);
	int save_fps = 24;
	int frameNum = 0;
	int count = 0;
	while(1)
	{
		IplImage *leftFrame = cvQueryFrame(bottomC);
		IplImage *rightFrame = cvQueryFrame(topC);
		if (!leftFrame || !rightFrame)
		{
			break;
		}
		frameNum++;
		if (frameNum%save_fps == 0)
		{
			char fileName[255] = {NULL};
			sprintf(fileName, "%d", count);
			string sf1 = string("save_image\\15_9\\bottom\\b2_") + string(fileName) + string(".png");
			string sf2 = string("save_image\\15_9\\top\\t2_") + string(fileName) + string(".png");
			count++;

			cvResize(leftFrame, leftShow);
			cvFlip(leftShow, leftShow, 1);
			cvResize(rightFrame, rightShow);
			cvShowImage("left", leftShow);
			cvShowImage("right", rightShow);			
			cvSaveImage(sf1.c_str(), leftShow);
			cvSaveImage(sf2.c_str(), rightShow);
			cvWaitKey(1);		
		}		
	}
	cvWaitKey(0);
	return 1;

}

#endif

#if 0
int main()
{
	double tt = clock();

	CvCapture *leftC  = cvCaptureFromFile("bottom_23345300.MXF");  //left.avi  bottom_23345300.MXF
	CvCapture *rightC = cvCaptureFromFile("top_23345209.MXF");     //          top_23345209.MXF
	
	CvxCapture *pCap = new CvxFileCapture(leftC);

	pCap->SetProperty(CVX_FILE_CAP_PROP_FPS, CVX_CAPTURE_60PI_PFS/4);
	pCap->SetProperty(CVX_FILE_CAP_PROP_HEIGHT, 1080/2);
	pCap->SetProperty(CVX_FILE_CAP_PROP_WIDTH, 1920/2);
	pCap->SetProperty(CVX_FILE_CAP_FLIP, 1);
	
	while(1)
	{
		double tt = clock();
		IplImage *img = pCap->GetNextFrame();
		if (img)
		{
			
			cvShowImage("left", img);
			cvWaitKey(1);
		}
		else
		{
			break;
		}		
		cout<<"cost time = "<<clock() - tt<<endl;
	}
	cvWaitKey(0);
	return 1;
}
#endif




